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Abstract 

A recursive algorithm is developed for the solution of the simulation dynamics problem for a chain 
of rigid bodies. Arbitrary joint constraints are permitted, that is, joints may allow translational and/or 
rotational degrees of freedom. The recursive procedure is shown to be identical to that encountered in a 
discrete-time optimal control problem. For each relevant quantity in the multibody dynamics problem, 
there exists an analog in the context of optimal control. The performance index that is minimized in the 
control problem is identified as Gibbs’ function for the chain of bodies. 


1 Introduction 

The need to predict the motion of robotic systems in terrestial and and space applications has focused 
attention on the area of multibody dynamics. In this paper, we treat the simulation dynamics of a chain 
of rigid bodies. Given the external force distribution and control influences acting on the chain, we show 
how its subsequent motion, namely, the joint accelerations, can be determined using a recursive procedure. 
The equations of motion and kinematical constraints constitute a two-point boundary value problem. The 
key to its solution is the elimination of the constraint forces which exist at each joint. Our method in this 
regard is a generalization of that used by FEATHERSTONE [1983] for single degree of freedom joints, although 
FEATHERSTONE [1987] has explained how the extension to general constraints can be effected. 

Recently, Rodriguez [1987] has pointed out the similarity between the equations describing a chain 
of hinged bodies and those that arise in discrete-time optimal estimation and smoothing problems. His 
approach has utilized the correspondence with optimal filtering (the Kalman filter) and smoothing (the 
Bryson- Frazier smoother). Here, we show that the equations are identical in form to an optimal control 
problem. In fact, there is a one-to-one correspondence between the elements of the multibody dynamics 
problem and the control problem. The feedback solution for the control in terms of the state is precisely that 
which yields the joint accelerations in terms of the body accelerations. The analogy is further uncovered by 
identifying the performance index (written in terms of the chain dynamics) as Gibbs’ [1879] function. 

The major benefit of a recursive solution of the simulation problem is its computational efficiency. 
One avoids dealing with the system of equations describing the system in its entirety. This would involve 
Gaussian elimination of the global mass matrix at each time step. The computational consequences of this 
can be quite substantial since the number of calculations involved in a recursive solution grows linearly with 
the number of bodies whereas the Gaussian elimination obeys a cubic relationship. 


2 Equations of Motion 

Let us consider a chain of contiguous bodies #o, Bi, . . . , Bn as shown in Figure 1. Interbody joints may 
permit arbitrary relative (rotational and/or translational) motion. Each joint therefore possesses at least 
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one degree of freedom and at most six. For convenience, we shall assume interbody translations to be small; 
however, the extension to large translations can be incorporated into the present formulation. For additional 
details on the derivation of the equations of motion, the reader should consult Sincarsin & Hughes [ 1989 ] 




Figure 1: A Chain of Rigid Bodies 


The motion of B n is defined by the velocity v n of O n and the angular velocity u> n of B n . (See Figure 
2.) Both v n and u> n are measured with respect to inertial space but are expressed in a reference frame 
attached to B n . We shall define 




A 


V n 


( 1 ) 


as the generalized velocity (cf. twist velocity) of B n at O n . We furthermore introduce the accompanying 
definition for a generalized force ( cf. wrench) acting at O n : 
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where 1 and g JJ 1 are the reaction forces and torques on B„ due to B n - 1 as expressed in T n . 


(2) 



0 n + 1 


Figure 2: Reference Frame 

The resulting equation of motion for B n can be written as 

Ad n V n — f n T fnl 


( 3 ) 
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where 


M n 


A 


m n 1 -c£ 

r x 7 
c n 


is the (constant) mass matrix corresponding to B n , that is, m n , c n and J n are the zeroeth (mass), first and 
second moments of inertia (about O n ) of B n . Also, f nT is the total external (generalized) force acting on 2? n , 
including interbody forces, and / n/ , which accounts for the nonlinear inertial terms, can be neatly written 
as 


fnl = {Vn) T M n V n 


( 4 ) 


where 



and (•)* operating on a Cartesian (3x1) column matrix, such as v„, u>„ or c n , is the matrix equivalent of 
the vector cross product. In a rate-linear model, one would set f n j = 0. 


Interbody Constraints 

The set of equations (3) does not yet describe a chain of bodies since it does not take into consideration the 
interbody constraints imposed by the joints. To do so, we begin by observing that 

^ n,n-l^n-l ,int (^) 

which introduces the relative interbody generalized velocity v n ,int of B n with respect to B n - l* In addition, 


n,n — 1 — 


Cn t n-1 l r n _i 

Cn.n -1 


is the generalized tranformation matrix between B n -i and B n ; C n n _i is the rotation matrix from Tn~\ 1° 
T n and is the position of O n with respect to O n - 1 - The geometric constraints imposed by the joints 
can thus be expressed formally as 

v n,int = 'Pn'Vny (®) 

where V n is a projection matrix and v ni is the column of free joint (rate) variables. The absolute velocities 
v n can be obtained recursively from and v ny . 

We also note that 

fnT ~ ^n + l.n/n+l ~ fn +fn t ex t 

where f n ext is due to solely to external influences. Furthermore, the generalized interbody forces /JJ ' 1 can 
be expressed as a sum of control forces f n c and constraint forces /„ □, tc., 

fT l = -Vnfn,* ~ Cn/n,D ( 8 ) 


The projection matrix Q n is the complement of P n . 


Projection Matrices 

A few words are perhaps in order regarding the projection matrices. First, as a simple yet very impor- 
tant example, consider a joint with a single rotational degree of freedom about, say, the third axis of an 
appropriately chosen reference frame. The corresponding projection matrix V n is 

T n = [ 0 0 0 0 0 1 ] T 

We may also add that v ny = 73 , where 73 is the angle of rotation. 

In general, V n is not constant, as above, but rather is dependent on configuration. Contemplation of 
a universal joint will quickly reveal this fact. The columns of T n are in general not orthonormal but 

TlT n =In ( 9 ) 
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where is X n is nonsingular. The complementary projection matrix Q n satisfies 

VlQn = o (10) 

Without loss in generality, the columns of Q n can be taken as orthonormal. 


Kinematical Equations 

The kinematical equations accompanying the dynamical equations (3) can be summarized in terms of T n n _ 1 


— v n,int^~n,n-l 


If we express v n int as 


we can extract from (11), 


A 

v n,int — 


v n,int 

^n,int 




( 12 ) 

(13) 


For physical reasons, Euler angles make for the most convenient and expedient representation of rotational 
joint degrees of freedom. Interbody translation is given by the integration of v„ ( i nt and would be reflected 
in T n-i- 


3 Rate-Linear Simulation Dynamics 

The recursive method presented here is a generalization of Featherstone ’s method applicable to rigid multi- 
body chains with arbitrary interbody constraints. The development, in fact, runs parallel to a similar 
generalization of Armstrong’s recursive method [D’Eleuterio 1989]. The essential difference is that the 
former is based on an affine relationship of the total interbody force to the absolute (generalized) body accel- 
eration while the latter relates explicitly only the interbody constraint force. The generalized Featherstone 
approach is particularly appealing because of its direct analogy to the discrete-time optimal problem. As 
shall be demonstrated, however, a simple equivalence exists between the two schemes. 

Let us begin, for explanatory purposes, by considering the rate-linear model, that is, we shall set 
f nI = 0 in (3) leaving 

A4 n v n = f nT (14) 

The extension to the nonlinear case (and, in fact, to elastic multibody trees) will be straightforward from 
here, although not totally without some algebraic effort. 

Recursion for /" _1 

We conjecture that the interbody forces can be written as 

- fn~ l - .(15) 

which is a generalization of Featherstone ’s hypothesis. Note that is, in effect, a mass matrix and i/> n is 
a generalized force quantity. The recursive algorithm is based on this result and the fact that ^ n and t/? n 
can be determined recursively from Bn to Bq. 

The proof of (15) is by induction: 


288 



Step I. For Bn, (14) becomes ^ 

MsVN = -f N + /jV,ext 

wherein it has been observed that 

f N + 1 = 0 

since Bn is the (free) terminal body. It is immediately obvious that if we set 

^ N — AAn > ”0 N f N,ex t 

(15) is satisfied for n = N . 


Step II. We assume that 


~ /n + l *“ ^n + l^n + 1 + V*n + 1 


Step III. Given (18), we shall show that (14) follows. Substituting (7) into (14) yields 

M n b n = Tl + 1 <n n +1 - /r 1 + /n,ext ( 19 ) 

Now, 

Vn + l = ^n+l,nVf» + 'Pn + l^n+1,7 (^0) 

and 

= ^n+l,n^n "b < ^n+l* , r»+ 1,7 ' ' 

(Note that the terms involving the time derivatives of T„+i |f i and P n+ i are omitted since they are nonlinear 
rate terms.) Substituting (21) and (8) in (19) and premultiplying by V* +1 gives 

2Vi+l/n + l,c = ®n + l,Ppi’n+l,7 + ^n + 1 ®n + l‘7'n+l,nt , n + V’n + l.P 

where, in general, 

9 nPP ±Vl* „V n , 1>nP = 'Pl'l’n 

Solving for t>„+i, 7 from (22), inserting back into (21) and using the result with (18) in (19) eventually leads 
to 

— f*n~ 1 — 4- n (^n + i - ^n+lPn+l^n + i^p^n + l^n+O^n+l.nJ^ft 

+ {T'n + l.n^n+l^+^n + l.Pft^ + ^n+l.c “ V’n+l.p) ^n+l] ” / n,ext) 

Hence, we can identify 

= A4 n + T^ +l n (^ n + l ” ^n + l'Pn+l^ n |i ) Pp'Pj'+l^n + l)7'n+l ) n (24) 

= < 7 F '^-j.i >n [^n+l7 > n + l^n + l,Pp(-^" + l/n + l,c ” ^n+l,p) d" V^n + l] “ /n,ext 

Step IV. By induction, then, (15) is proven. D 

The matrix has an attractive physical interpretation. It is the mass matrix (about O n ) of the 
part of the chain from B n to Bn associated with the constrained degrees of freeedom. Featherstone 
[1983] would refer to as the art;cu/ated-body inertia. It should also be pointed out that which is 
positive-definite, and tl> n are configuration-dependent. 


Recursion for v ni 

By the inductive nature of the proof for (15), it has been shown that the matrices and V> n can be 
evaluated recursively inward, i.e., from Bn to Bo. Having done so, one can then perform outward recursion, 
from Bo to Bn , to solve for v ni . This is evident from (22). 

Rewriting (22) for B n instead of R„+i and solving explicitly for v ny yields 

Vny ~ ^nPr(^ n /n,c ~ ^ n ® n^n,n- l^n— 1 — ^r»p) 

Examining this result, we see that at B n all the quantities on the right-hand side are known since v n -i can 
be computed recursively from its inboard neighbor according to (21). 
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4 Nonlinear Simulation Dynamics 


The extension to the nonlinear case can be had by simply persevering with the nonlinear rate terms in 
the preceding development. However, there is a much more palatable approach which is also not without 
significance in computational considerations. 

Let [Golla 1988] 

= d n ®n, non (26) 

such that 

— 1 ”b n^ny (27) 

Inserting (27) into (5) and differentiating reveals that we must have 

®n,non = 2~n,n- 1 1 ,non ^n,n--l v n — 1 + 'Pn v ny (28) 

for (28) to hold. In essence, the acceleration quantities a n account for the rate-linear effects and a n>non for 
the nonlinear effects. Moreover, not only is a„ found recursively (outward) but a n)n0 n as well. 

Upon substitution of (27) into the motion equation (3), we have 


f nT d" f nl f n f non 


(29) 


where 


In fact, we could write (30) as 


f n,non 


— n 


M n a n = Tj +lfn /S +1 - + /„ 


(30) 


where 


f n,net / n,ext + fnl + / n,n 


Comparing (31) to (19), we learn that the nonlinear dynamics model is of the identical form as the rate-linear 
model with i) n replaced by a„ and ext replaced by / nnet . We can therefore apply the results obtained 
above directly to the nonlinear case. 


Recursion for 1 

In general, then, for rigid multibody chains 

-/r X = *» «n + P„ (31) 

Note that is the same as before; however, 

Pn — + + l ^n + l,Pp(^ n +l/ n+l,c l,p)fPn + l] f n,net (32) 

with 

PnP = rlp n 

and p N = —/^net- 


Recursion for i) ni 

The recursive relation for v ny can be expressed as 

= nppi^nf n,c ~~ ^ n ^ n,n-l a n-l — Pnp) (33) 

which reflects (26). It bears mentioning that the kinematical equations remain unchanged. 
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Relationship to Armstrong’s Work 

Before proceeding onward, it is worth pointing out that 

- * n V n *-pp'Pl*n = Qn*nQ T (34) 

where 

*n = *nQQ ~ *Z P Q*nPP*nPQ 

and 

'&nPQ — 'Pn'&nQn, ^nQQ = fil^nCn 

Showing (34) requires invoking the identity 

V n T- l Vl + Q n Ql = 1 

By virtue of (34), we can rewrite the first of (24) as 

= + ^n + l,n®ri+l ^n + 1 G n +l^n+l,n 

which is a more streamlined expression. 

The significance of $ n , however, lies in the fact that 

fn,a = *nQla n + <f> n (36) 

where 

<t>n = CnPn + *lpQ*nPP&r>fn,c ~ Pnp) 

This result is equivalent to Armstrong’s method for rigid multibody chains with arbitrary joint constraints. 

5 A Discrete-Time Optimal Control Problem 

Diverting our attention from multibody dynamics momentarily, consider the following optimal control prob- 
lem: minimize 

J = ixJ’Mtx* + x*h* - (37) 

it=o 1 

subject to the linear state equation 

Xfc+i = Atxt + BjfcUjt, x_i = 0 (38) 

Here, M* is a sequence of positive-definite weighting matrices, and hi and t k are vector weighting sequences. 
Since ujv does not influence x*, k < N, we shall assume that t N = 0. This problem is slightly different than 
the standard “linear-quadratic” version that one typically encounters. The cost functional in the present 
case is linear in the control variable. 

Minimizing J subject to the state equation is a straightforward optimization problem. Introducing 
the lagrange multiplier or adjoint variable X k , we define the augmented performance index as follows: 

J' = y* ixjMjfcXjt + x^hjt - + A* (x t — Aj k _ix fc _ 1 - Bi-iu^,) (39) 

Jfc = 0 

The necessary conditions for optimality, 

8J' _ b3> _ b3> _ Q 
d\ t+i dx k du k 
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produce the two-point boundary value problem (tpbvp): 

x* + i = AjfcXjb + B k n k , x_! = 0 (40) 

X k = Aj’Ajb+x — MfcXjfc — hjk , Ajv+i = 0 (41) 

tjb = — B^Ajt +1 (42) 

We have taken Aat+i = 0, without loss in generality, since tjv = 0. Hence, from (41), Aat = ~M n xn - 
which supplies the basis for the inhomogeneous Riccati transformation, sometimes called the sweep method: 

A*. - -Sjtxjt - rjb (43) 

with S;v = M tv and r^r = h;y. Substituting (43) into the equation for t* (42) and replacing x*+i with the 
right side of (40), produces the feedback law 

U* = -K*x t + R* x [t* - B* r t+ i] (44) 

where 

R* ^ BjS* +1 Bjt , K t i TL^BjS k+1 A k 

The matrix R* will be invertible if Bjb is monic and S* + i is positive-definite. Substituting the sweep solution 
(43) for A* and A*+i and using (40) for xjt+i and (44) for u* gives 

[S* — Aj^Sjfc+i — Sfc+iBfcR^B^Sfc+^Ajt — MjkJxjfc 

= — r k + (Ajt — B t Kjt) T rjt +1 + K^t* + h* 

Since this must hold for general x*, the coefficient of x* must vanish as well as the right hand side. Hence, 

Sfc = A^(Sjt + i — S ( fc +1 BjtR^ 1 Bj’Sfc + i)Ajt -I- M* (45) 

which is the discrete-time matrix Riccati equation and 

ft = (A* - B*K*) T rjfc+i + Kjfct* -f h* (46) 

We now return to the question of the invertibility of R*. The definitions of K* and Rjt reveal that ( A k — 
BjfcKjfc) T Sjfc + 1 Bfc = 0 which allows us to write the Riccati equation as 

S* = (A* - B*K*) t S* + i(A* - B^Kjfe) + Mjb (47) 

Since S;v = M// is symmetic and positive- definite, S* is symmetric and positive-definite (using backwards 
induction). Hence, R* defined previously is positive-definite and is always invertible. 

The optimal control policy can now be summarized as follows: one solves the Riccati equation (45) (or 
(47)) and the vector equation (46) backwards from k = N to k = 0 using the boundary conditions S^y = 
and r^v = h/y. The optimal control can then be calculated using (44) while propagating the state forward 
using the state equation (40). 

6 Relationship Between Optimal Control and Recursive Dynam- 
ics 

The TPBVP generated by the previous optimal control problem (40-42) is identical in form to that of the 
multibody dynamics problem (30), (33), and 

= -Vlr n - 1 (48) 

which follows from premultiplying (8) by while recognizing (9) and (10). Therefore, we make the following 
identifications: 


X* 

* ► 

A* 

4 ► /"- 1 

/ n 

Ui 

* * ^n + 1,7 

h t 

i * - f n,net 

A* 

* * ^n+l p n 

Mt 

4 — * 

B* 

4 ► V n + 1 

ti 

4 * -^n+l/n + l,c 


292 



Hence, the accelerations a„ are analogous to the states, the interbody forces 1 are analogous to the 
adjoint states, the joint accelerations play the role of the control inputs, and the projection matrices 
T n +i take the place of the input matrix B k . It can be shown that the interbody tranformation matrices 
T^+i „ possess the properties of the state transition matrix thus completing the analogy. Comparing the 
transformation (43) with the generalization of Featherstone’s solution (32) allows us to identify 

S* < — * , r k < — » p n , R* ♦ — * *n+i ,PP 

We also emphasize that recursion in time ( k ) has been replaced by spatial recursion (n) at a given instant 
in time. 

Using the above identifications, the performance index J can be written as 

N 1 

J = 2 a ^^ n(ln ~ ~ /n, 

n=0 Z 

Hence, in the multibody dynamics problem one can minimize J subject to the kinematical constraint equa- 
tion (30) to arrive at the defining equations. Compare this with Gibbs’ [1879] formulation of the dynamics of 
a system of N particles with masses m n , coordinates x n ,y n ,z n , and subjected to forces X n ,Y n ,Z n : minimize 

£ 2 + Vn + *n) — X n X n ~ Yntin ~ Z n Z n 
n = 1 

subject to the kinematical constraints. 

In the work of Rodriguez [1987], he points out the similarity between the equations describing a chain 
of hinged bodies and the tpbvp that arises in discrete- time, optimal estimation and smoothing problems. 
In his formulation, the bodies in the chain are numbered inwardly (t.e., the tip body is Bo and the root 
body is Bn)- Here, the numbering of the bodies is outward (the root body is Bo and the tip body is Bn). 
With this convention, the equations are rendered dual to those of Rodriguez. As such, the corresponding 
discrete-time problem is not one of estimation and smoothing but one of control. It is interesting to note the 
dual relationships inherent in Rodriguez’s work. The role of the state is played by the interbody forces and 
the adjoint states are the link accelerations, which are a juxtaposition of the results given above. The control 
torque at each joint plays the role of a measurement of the states whereas we have the joint accelerations 
acting as ‘control inputs’. 

7 Summary of the Recursive Algorithm 

now summarize the procedures for determining the motion of the chain of bodies. The control forces, 
f n c (t), and external force distribution, /„ ext (<), are prescribed on the time interval of interest. Beginning 
with t = 0, we proceed as follows: 


Step 1. At time t, the relative velocities w„,(<) and the rotation matrices C n _„_i(t) are known. 

Step 2. Outward recursion for the velocities v n and determination of /„ net : 

Do n = 0 to N\ 

Generate T n;n _ i using C n n _i. 

v n,int — 


T~n,n — 1 — 


n>n- 


— ^n,n — l^n — 1 d" v n, 


1- 

,int • 


■*n,non 


: — l®n — l,non -bT~ n ,n — iT’n — 1 d~ n ^ 


f nl — ( v n ) f n,non “ 

f n,net f n.ext d~ f n j T f ^ 

Next n. 


ri7- 


293 


Step 3. Inward Recursion for and p n ; 

Set *at = M n and p N = -/^ >non - 
Do n = N — 1 to 0; 

*&n + l,PP — 7*11 + 1 ^ ri+l'Pn + l, P n + l,P = ^n+lPn+1 

= ^n + l,PP^ n+l^n+l^"n+l,m ^n + l.n — ^n + l,n ^nV n+1- 

= r£ +1 ^ n+ ir n+ i, n 4* Ai n 

Pn ^n|l,nPn+l d“ ^n ^ n+l,c f n,net 

Next n. 

If Vo ^ O, ^opp = Vq ^o'Po, Pop = 'Po Po 

Step 4. Outward Recursion for v ni : 

If Vo = O (Bo is constrained), then vo-y = ao = 0 
Otherwise, ho-r = *opj>[/o,c ~ V’o p}> a o = ’PoVoy- 
Do n = 1 to N; 

V n y = —JCn-l a n-l + ® n ,Pp(f n,c — Pnp) 

C n ,n-1 — “^n.int ^n,n— 1 

— ^n,n— l a n— 1 4" V n^ny 

Next n. 

Step 5. Estimate v nl (t 4- Af), C n n _i(< 4* Af) using some quadrature scheme. 

Go back to Step 1 and replace t with t 4* At. 

This completes the summary of the recursive simulation procedure. Note that in a rate-linear simulation, one 
ignores the contributions of f nI and f n non to / n net in Step 2. We have written the recursion for 4> n and 
p n , in Step 3, in terms of the quantities K n and r n+ i n since this leads to the most compact and efficient 
expressions. The fourth step produces the joint acclerations v ni which can be integrated in conjunction 
with the kinematical relationships for the rotation matrices to produce the joint orientations/positions and 
velocities. 


8 Concluding Remarks 

Given the forces on a chain of rigid bodies, we have shown that the accelerations of the bodies can be 
determined using the recursive procedures of discrete-time optimal control. The underlying analogy that 
makes this possible yields great insight into the structure of the multibody dynamics problem. 

There are many extensions of the present results, a few of which we shall briefly mention here. The 
analysis presented was limited to topological chains of rigid bodies. It is easily extended to topological 
tree configurations. The problem of flexible multibody dynamics has been considered by D’Eleuterio 
[1989] who shows that the structure of the equations is unaltered by flexibility. Indeed there is a one-to-one 
correspondence between the rigid and flexible problems. With this duality in hand, one can readily extend 
the present analysis to the problem of elastic multibody chains. Such an extension has been performed by 
Damaren & D’Eleuterio [1989]. 
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